//
// Created by xujingyi on 2021/5/8.
//
#include <string>
#include <algorithm>
#include <opencv2/calib3d.hpp>
#include "armor_finder.h"
//#include <opencv2/core.hpp>
//#include <opencv2/highgui.hpp>
#include "Setter.h"
#include "log.h"
#include "additions.h"
#include "show_images.h"
#include "systime.h"

extern Serial serial;
extern rm::Setter rm_setter;
extern int gSrcNum;
// 判断灯条颜色(此函数可以有性能优化).
rm::ArmorColor get_marker_color(const cv::Mat &src, const cv::RotatedRect &blobPos) {
    auto region = blobPos.boundingRect();
//    region.x -= fmax(3, region.width * 0.1);
//    region.y -= fmax(3, region.height * 0.05);
//    region.width += 2 * fmax(3, region.width * 0.1);
//    region.height += 2 * fmax(3, region.height * 0.05);
    region &= cv::Rect(0, 0, src.cols, src.rows);
    cv::Mat roi = src(region);

    cv::circle(src,region.tl(),1,cv::Scalar(0,255,255),-1);
    cv::circle(src,region.br(),1,cv::Scalar(0,255,255),-1);
    int red_cnt = 0, blue_cnt = 0;
    int total_num=roi.rows*roi.cols;
    for (int row = 0; row < roi.rows; row++) {
        for (int col = 0; col < roi.cols; col++) {
            red_cnt += roi.at<cv::Vec3b>(row, col)[2];
            blue_cnt += roi.at<cv::Vec3b>(row, col)[0];
        }
    }

    if ((red_cnt > blue_cnt)&&(((red_cnt-blue_cnt)/float(total_num))>30)) return rm::RED;
    if((blue_cnt > red_cnt)&&(((blue_cnt-red_cnt)/float(total_num))>30)) return rm::BLUE;
    else return rm::PURPLE;
}

extern cv::Mat origin,pre_pic,binary_pic;
//cv::Mat rm::ArmorFinder::preProcess(const cv::Mat &src) {
//    systime t1,t2;
//    getsystime(t1);
//    if(src.empty()){
//        LOGE("src used for preProcess is empty!");
//        return cv::Mat();
//    }
//    cv::Mat frame=src.clone();
//    show_pic=src.clone();
////    cv::imshow("TEST_PREPROCESS", src);
////    cv::waitKey(0);
//    ArmorColor enemy_color=getEnemyColor();
//    if(enemy_color==BLUE) {
//        param=new BlueArmorParam();
//    }
//    else if(enemy_color==RED) {
//        param=new RedArmorParam();
//    }
//    cv::Mat gray_img,binary_brightness_img;
//    cv::cvtColor(frame, gray_img, cv::ColorConversionCodes::COLOR_BGR2GRAY);
//    // 环境亮度是显著因素之一
//    cv::threshold(gray_img, binary_brightness_img, param->brightness_threshold_val, 255, 0);
//
//    std::vector<cv::Mat> channels;       // 通道拆分
//    cv::split(src, channels);
//    cv::Mat binary_enemycolor_img;//enemycolor
//    cv::Mat color_channel;
//    if(getEnemyColor()==BLUE) color_channel=channels[0];
//    else if(getEnemyColor()==RED) color_channel=channels[2];
//    cv::threshold(color_channel, binary_enemycolor_img, param->color_channel_threshold_val, 255, 0);  //200
//
//    cv::Mat enemy_color_diff;
//    cv::Mat binary_colordiff_img;
//    if(enemy_color==rm::BLUE) cv::subtract(channels[0], channels[2], enemy_color_diff);
//    else if(enemy_color==RED) cv::subtract(channels[2], channels[0], enemy_color_diff);
//
//    cv::threshold(enemy_color_diff, binary_colordiff_img, param->color_diff_threshold_val, 255, 0);  //200
//
//    binary_pic=(binary_brightness_img&binary_enemycolor_img)&binary_colordiff_img;
//
////    pre_pic=binary_pic.clone();
////    cv::Mat dst=binary_pic.clone();
//
////    cv::Mat dilate_element,dilate_pic;
////    int dilate_size=1;
////    dilate_element = getStructuringElement(cv::MORPH_RECT, cv::Size(dilate_size*2+1, (1+dilate_size)*2+1));
////    dilate(binary_pic, dilate_pic, dilate_element);
//
//    pre_pic=binary_colordiff_img;
//    cv::Mat dst=binary_pic.clone();
//    getsystime(t2);
//    LOGT("preprocess:%.1lfms",getTimeIntervalms(t2,t1));
//    return dst;
//};

cv::Mat rm::ArmorFinder::preProcess(const cv::Mat &src) {
    systime t1,t2;
    getsystime(t1);

    if(src.empty()){
        LOGE("src used for preProcess is empty!");
        return cv::Mat();
    }
    cv::Mat frame=src.clone();
//    cv::imshow("TEST_PREPROCESS", src);
//    cv::waitKey(0);
    ArmorColor enemy_color=getEnemyColor();
    if(enemy_color==BLUE) {
        param=new BlueArmorParam();
    }
    else if(enemy_color==RED) {
        param=new RedArmorParam();
    }
    cv::Mat gray_img,binary_brightness_img;
    cv::cvtColor(frame, gray_img, cv::ColorConversionCodes::COLOR_BGR2GRAY);
    // 环境亮度是显著因素之一

    cv::threshold(gray_img, binary_brightness_img, param->brightness_threshold_val, 255, 0);
    std::vector<cv::Mat> channels;       // 通道拆分
    cv::split(frame, channels);
    cv::Mat color_channel;

    cv::Mat enemy_color_diff;
    cv::Mat binary_colordiff_img;

    if(enemy_color==rm::BLUE) cv::subtract(channels[0], channels[2], enemy_color_diff);
    else if(enemy_color==RED) cv::subtract(channels[2], channels[0], enemy_color_diff);

    cv::threshold(enemy_color_diff, binary_colordiff_img, param->color_diff_threshold_val, 255, 0);  //200
    binary_pic=binary_brightness_img|binary_colordiff_img;
//    pre_pic=binary_pic.clone();
//    cv::Mat dst=binary_pic.clone();

//    cv::Mat dilate_element,dilate_pic;
//    int dilate_size=1;
//    dilate_element = getStructuringElement(cv::MORPH_RECT, cv::Size(dilate_size*2+1, (1+dilate_size)*2+1));
//    dilate(binary_pic, dilate_pic, dilate_element);

    pre_pic=frame;
    cv::Mat dst=binary_pic.clone();
    getsystime(t2);
    LOGT("preprocess:%.1lfms",getTimeIntervalms(t2,t1));
    return dst;
};

std::vector <rm::Marker> rm::ArmorFinder::findMarker(const cv::Mat &track_src,const cv::Mat &img2show)//找灯条
{
    if(track_src.empty()) return Markers();
        systime t1,t2;
        getsystime(t1);
    std::vector<std::vector<cv::Point>> contours;
//    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(track_src, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    Markers maybe_enemy_markers;
    for (int i=0;i<contours.size();i++) {
        cv::RotatedRect rect = cv::minAreaRect(contours[i]);
        cv::Mat vertices;
        cv::Size2f size=rect.size;
        boxPoints(rect, vertices);       //计算出的4个顶点存在一个4行2列的Mat对象中，每一行代表一个顶点坐标
        accuracy peri = arcLength(vertices, true);    //   首尾相连
        accuracy height_width_ratio=size.height>size.width?(size.height/size.width):(size.width/size.height);
        //circle("circle",Point2d(rect.center.x, rect.center.y);
        ArmorColor confirm_color=get_marker_color(img2show,rect);
        // && confirm_color!=PURPLE&&confirm_color==getEnemyColor()
        if ((peri > param->marker_min_peri) && (peri < param->marker_max_peri) &&(height_width_ratio>param->marker_min_height_width_ratio)&&(height_width_ratio<param->marker_max_height_width_ratio)&& confirm_color!=PURPLE&&confirm_color==getEnemyColor())
            maybe_enemy_markers.emplace_back(rect, peri, confirm_color);
    }
    //sort marker by x
    if (maybe_enemy_markers.size() >=2) {
        sort(maybe_enemy_markers.begin(), maybe_enemy_markers.end(), [&](const rm::Marker &a, const rm::Marker &b) {//按照x从大到小方式排序
                 return a.center().x < b.center().x;
             }
        );
    }

    if(rm_setter.show_markers){
        showMarkers(img2show,maybe_enemy_markers);
    }
    getsystime(t2);
    LOGT("findMarker:%.1lfms",getTimeIntervalms(t2,t1));
    return maybe_enemy_markers;
};

//判断装甲版大小
rm::ArmorSize judgeBoxSize(const bool is_blind,const rm::Marker &left,const rm::Marker &right){
    rm::ArmorSize boxsize;
    float max_angle_diff=std::max(abs(left.angle()-90.0),abs(right.angle()-90.0));
    accuracy length=std::max(left.size().x,right.size().x);
    float coef=(cos(max_angle_diff*CV_PI/180.0f));
//float coef=1;
    double ratio;
    if(length==0) ratio=0;
    else ratio=(getPointLength(left.center(),right.center())/coef)/ length;
//    if(abs(serial.rxpackage.pitch_speed.type_short))>2/3*rm_setter.cruise_judge_speed ratio*=1.2;
    if(serial.rxpackage.yaw_speed.type_short>2/3*rm_setter.cruise_judge_speed) ratio/=1.2;
    if(serial.rxpackage.chassis_speed.type_short>700) ratio/=1.2;
    if(is_blind) ratio/=2.0f;
    if(rm_setter.marker_min_distance_height_ratio<ratio && ratio<rm_setter.marker_middle_distance_height_ratio) boxsize=rm::SMALL_BOX;
    else if(rm_setter.marker_middle_distance_height_ratio<ratio && ratio<rm_setter.marker_max_distance_height_ratio) boxsize=rm::BIG_BOX;
    else
        boxsize=rm::NOT_BOX;
    return boxsize;
}

float lengthRatioJudge(const rm::Marker &l_marker, const rm::Marker &r_marker){
    if(l_marker.size().x==0||r_marker.size().x==0)
        return 0;
    float ratio = std::max((l_marker.size().x/r_marker.size().x),(r_marker.size().x/l_marker.size().x));
    if(ratio<rm_setter.couple_max_lengths_ratio) return 1;
    else return 0;
}

float angleJudge(const rm::Marker &l_marker, const rm::Marker &r_marker){
    float angle = abs(l_marker.angle()-r_marker.angle());
    if(angle<rm_setter.couple_max_angle_diff) return 1;
    else return 0;
}

float markerNBoxAngelJudge(const rm::Marker &l_marker, const rm::Marker &r_marker){
    cv::Point2f v=r_marker.vertexes()[0]-l_marker.vertexes()[3];
    float box_angle=(atan2(v.y,v.x)>CV_PI?(2*CV_PI-atan2(v.y,v.x)):(-atan2(v.y,v.x)))*180/CV_PI;
    float marker_box_angle_diff=std::min(abs(l_marker.angle()-box_angle),abs(r_marker.angle()-box_angle));;
    if ((marker_box_angle_diff>rm_setter.couple_min_marker_box_anglediff)&&(marker_box_angle_diff<(180-rm_setter.couple_min_marker_box_anglediff))) return 1;
    else return 0;
}


const int try_time=1;
rm::MarkerCouples rm::ArmorFinder::matchMarkerCouple(const std::vector <rm::Marker> &markers,const cv::Mat& img2show){
    if(markers.size()==0) {LOGW("find NO MARKER!");return MarkerCouples();}
    systime t1,t2;
    getsystime(t1);
    MarkerCouples maybe_marker_couples;
    if(markers.size()>=2){
        int try_num=try_time;
        for(int i=0;i<markers.size();i++){
            try_num=try_time;
            for(int j=1;j<try_num+1;j++){
                if(i+j<markers.size()){
            rm::Marker left=markers[i],right=markers[i+j];
            rm::ArmorSize size = judgeBoxSize(false,left,right);
            if((abs(left.center().y - right.center().y)/(std::min(left.size().x,right.size().x)>0?std::max(left.size().x,right.size().x):100)>0.4)){//如果以灯条长度为参照对象，两根灯条dy差别过大
                if (i+j+1<markers.size()+1) {try_num++;}//则很有可能不是灯条，所以再往更远的地方（x更大）寻找一条灯条，前提是还存在x更大的灯条
            }
            if((abs(left.center().y - right.center().y)/(std::min(left.size().x,right.size().x)>0?std::max(left.size().x,right.size().x):100)>2)) continue;//如果以灯条长度为参照对象，两根灯条dy差别过大,pass

            float total_score =
            lengthRatioJudge(left, right) +
            angleJudge(left, right)+
            markerNBoxAngelJudge(left, right);

//            LOGM("%d,%d: length_ratio:%f angle_diff:%f,mk_bx_ang_dif:%f   dy/height:%f  dis/height:%f  try_num:%d",i,i+j,lengthRatioJudge(left, right),angleJudge(left, right),markerNBoxAngelJudge(left, right),
//                    (abs(left.center().y - right.center().y)/(std::max(left.size().x,right.size().x)>0?std::max(left.size().x,right.size().x):100)),
//                 abs(left.center().x-right.center().x)/ ((left.size().x==0)?0.0001:std::min(left.size().x,right.size().x)),try_num);
            if(left.color()==right.color()&&total_score >= rm_setter.pass_score &&size!=rm::NOT_BOX&& left.color()!=PURPLE&&left.color()==getEnemyColor())
                maybe_marker_couples.emplace_back(left,right,size);
            float move_ratio=1.0f;
            if(std::max(abs(serial.rxpackage.pitch_speed.type_short),abs(serial.rxpackage.yaw_speed.type_short))>400) move_ratio*=2;
            if((abs(left.center().x-right.center().x)/ ((left.size().x==0)?0.0001:std::max(left.size().x,right.size().x))>move_ratio*rm_setter.marker_max_distance_height_ratio)) break;//
                }
            }
        }
    }
        for(int i=0;i<markers.size();i++) cv::putText(img2show,std::to_string(i),markers[i].center()+cv::Point2f(-20,-20),1,1,cv::Scalar(255,255,255),1.5);
        if(rm_setter.show_marker_couples) showMarkerCouples(img2show,maybe_marker_couples);

    getsystime(t2);

    if(maybe_marker_couples.size()==0) LOGW("FIND NO COUPLE!!");
    LOGT("matchMarkerCouple:%.1fms",getTimeIntervalms(t2,t1));
    return maybe_marker_couples;
};

extern cv::Mat warp_img;
extern std::string Num_Path1,Num_Path2,Wrong_Path,Miss_Path;

//仿射变换 Point2f类型不要改！
cv::Mat boxPerspectiveTransform(const cv::Mat &img,const rm::MarkerCouple &couple,const cv::Mat&src){
    if(img.empty()) return cv::Mat();
    systime t1,t2;
    getsystime(t1);

    std::vector<cv::Point2f> box_vertexes=couple.vertexes();
    cv::Rect2f bounding_rect;

    bounding_rect=boundingRect(box_vertexes);//只是围住了灯条点，但是数字会框不完全
    //enlarge boundingrect
    accuracy blob_len=std::max(couple.left_mark.size().x,couple.right_mark.size().x),blob_dis=getPointLength(couple.left_mark.center(),couple.right_mark.center());
    bounding_rect = bounding_rect - cv::Point2f(0,blob_len*(rm_setter.perspect_num_box_plus_ratio/2.0)); //平移，也就是左上顶点的xbubian，y坐标+100
    bounding_rect = bounding_rect + cv::Size2f(0, blob_len*rm_setter.perspect_num_box_plus_ratio);  //缩放，左上顶点不变，宽度-100，高度+100

    cv::Point2f deviation_point;
        deviation_point= bounding_rect.tl();//box_vertexes偏移量,用于仿射变换
//		bounding_rect=bounding_rect - deviation_point;//boundingrect偏移到左上角
    for(std::vector<cv::Point2f>::iterator it=box_vertexes.begin();it!=box_vertexes.end();++it){
        *it-=deviation_point;
//        cv::circle(show_pic,*it,3,cv::Scalar(0,0,255),-1);
    }

    std::vector<cv::Point2f> refer_point2fs;
//	getProperReferPoints(refer_point2fs,boxsize,img.size);
    accuracy refer_width,refer_height;
    if (couple.boxsize() == rm::BIG_BOX ) {
        refer_width = BIG_ARMOR_BOX_W;
        refer_height = BIG_ARMOR_BOX_H;

    }
    else if(couple.boxsize() == rm::SMALL_BOX ) {
        refer_width = SMALL_ARMOR_BOX_W;
        refer_height = SMALL_ARMOR_BOX_H;
    }
    else LOGE("Couple size is wrong!!");

//	Point2f center=cv::Point2f(img_size()/2);0
    cv::Point2f center=cv::Point2f(PICTURE_COL_NUM,PICTURE_ROW_NUM);

    accuracy coef=blob_len/float(refer_height);
    refer_width*=coef;
    refer_height*=coef;

    refer_point2fs.emplace_back(cv::Point2f(0, refer_height));//左下
    refer_point2fs.emplace_back(cv::Point2f(0 , 0));//左上
    refer_point2fs.emplace_back(cv::Point2f(refer_width , 0));//右上
    refer_point2fs.emplace_back(cv::Point2f(refer_width, refer_height));//右下

//	Point2f src_points[4];
//	Point2f dst_points[4];
//    cv::Mat rotation = getPerspectiveTransform(box_vertexes, refer_point2fs);  //计算透视变换矩阵
//    (0,0)   _________________  (w,0)
//           |                 |          P=[0,w,w,0;
//           |                 |             0,0,h,h;
//           |                 |             1,1,1,1]
//           |_________________|
//     (0,h)                      (w,h)
    // typedef Mat_<float> cv::Mat1f
    cv::Mat1f rotation;
    cv::findHomography(box_vertexes, refer_point2fs, 1).convertTo(rotation,CV_32F);
    cv::Mat1f H=rotation;
//  std::cout<<rotation<<std::endl<<std::endl<<H<<std::endl<<std::endl;
//  defined and enlarge the bounding-markerCOUPLE-rect's height to armorbox's size
    cv::Mat1f p =(cv::Mat_<float>(3,4)<<0.,refer_width,refer_width,0.,-0.5*rm_setter.perspect_num_box_plus_ratio*refer_height,(-0.5*rm_setter.perspect_num_box_plus_ratio*refer_height),refer_height*(1+0.5*rm_setter.perspect_num_box_plus_ratio),refer_height*(1+0.5*rm_setter.perspect_num_box_plus_ratio),1.,1.,1.,1.);

    if(rotation.empty()) return cv::Mat();
    cv::Mat1f P=rotation*p;
    cv::Mat1f min_cols,max_cols;
    cv::reduce(P, min_cols, 1, cv::REDUCE_MIN);
    cv::reduce(P, max_cols, 1, cv::REDUCE_MAX);
//    std::cout<<P<<std::endl<<std::endl;

    float minx=min_cols.at<float>(0),miny=min_cols.at<float>(1);
//    std::cout<<minx<<" "<<miny<<std::endl;
    float maxx=max_cols.at<float>(0),maxy=max_cols.at<float>(1);

    rotation-=(cv::Mat_<float >(3,3)<<0,0,minx<0?minx:0,0,0,miny<0?miny:0,0,0);
//    std::cout<<rotation<<std::endl;
//    cv::Size2f warp_size=cv::Size2f((maxx-minx),(maxy-miny)*(1+setter.perspect_num_box_plus_ratio));
    cv::Size2f warp_size=cv::Size2f((maxx-minx),(maxy-miny)*(1+rm_setter.perspect_num_box_plus_ratio*0.5));
    if(warp_size.height>500||warp_size.width>500) {
        LOGW("WARP SIZE too large") ;
        return cv::Mat();}

    cv::Rect2f temprect=bounding_rect & cv::Rect2f(0, 0, img.cols, img.rows);
    cv::rectangle(src,bounding_rect, cv::Scalar(0, 0, 255),1);//bounding_rect in box place
    if (temprect.empty()) return cv::Mat();

    cv::Mat tempimg=img(temprect).clone();

    warpPerspective(tempimg,warp_img, rotation,warp_size,cv::INTER_NEAREST | cv::WARP_FILL_OUTLIERS);
    cvtColor(warp_img,warp_img, cv::COLOR_BGR2GRAY);
    equalizeHist(warp_img,warp_img);
//    warp_img*=(float)30.0;
//    imshow("perspect_point",warp_img);
//    showMarkerCouple(show_pic,couple);
//    cv::imshow("show_pic",show_pic);
////    cv::waitKey(0);
//    if (couple.boxsize() == rm::BIG_BOX ) {
//        cv::Rect2f roi= cv::Rect2f(cv::Point2f(warp_img.cols*0.25,0),cv::Point2f(0.75*warp_img.cols,warp_img.rows))&cv::Rect2f(3,3,warp_img.cols,img.rows);
//        cv::rectangle(show_pic,roi, cv::Scalar(0, 255, 0),1);
//        if (roi==cv::Rect2f()){return cv::Mat();};
//        warp_img=warp_img(roi);
//    }

    cv::Mat num_img=warp_img.clone();
    if(rm_setter.save_num_picture){
        static int i=0;
        if((gSrcNum%rm_setter.save_internal)==0){
            std::string num;
            num=Num_Path1+"/picture_"+ std::to_string(gSrcNum) +".jpg";
            cv::imwrite(num,warp_img);

            cv::Mat ori=20*origin(bounding_rect & cv::Rect2f(0, 0, src.cols, src.rows));
            num=Num_Path1+"/picture_"+ std::to_string(gSrcNum) +"ori.jpg";
            cv::imwrite(num,ori);
        }
    }

    threshold(num_img, num_img, rm_setter.num_threshold, 255, cv::THRESH_BINARY);
//    medianBlur(num_img, num_img, 2 * 1 + 1);

    if(rm_setter.save_num_picture){
        if((gSrcNum%rm_setter.save_internal)==0){
            std::string num;
            num=Num_Path2+"/picture_"+ std::to_string(gSrcNum) +"num.jpg";
            cv::imwrite(num,num_img);
        }
    }

    getsystime(t2);
//    LOGT("boxPerspectiveTransform:%.1fms",getTimeIntervalms(t2,t1));
    return warp_img;
}

////仿射变换 Point2f类型不要改！chongxie1
//cv::Mat boxPerspectiveTransform(const cv::Mat &img,const rm::MarkerCouple &couple){
//if(src.empty()) return cv::Mat();
//    systime t1,t2;
//    getsystime(t1);
//
//    std::vector<cv::Point2f> box_vertexes=couple.vertexes();
//    cv::Rect2f bounding_rect;
//
//    bounding_rect=boundingRect(box_vertexes);//只是围住了灯条点，但是数字会框不完全
//    //enlarge boundingrect
//    bounding_rect = bounding_rect - cv::Point2f(0,bounding_rect.height*(setter.perspect_num_box_plus_ratio/2.0)); //平移，也就是左上顶点的x坐标-100，y坐标+100
//    bounding_rect = bounding_rect + cv::Size2f(0, bounding_rect.height*setter.perspect_num_box_plus_ratio);  //缩放，左上顶点不变，宽度-100，高度+100
//
//    cv::Point2f deviation_point;
//    if(couple.left_mark.angle()<=90)
//        deviation_point= bounding_rect.tl();//box_vertexes偏移量,用于仿射变换
//    else
//        deviation_point= cv::Point2f(box_vertexes[0].x,box_vertexes[1].y);
////		bounding_rect=bounding_rect - deviation_point;//boundingrect偏移到左上角
//    for(std::vector<cv::Point2f>::iterator it=box_vertexes.begin();it!=box_vertexes.end();++it){
//        *it-=deviation_point;
//        cv::circle(show_pic,*it,3,cv::Scalar(0,0,255),-1);
//    }
//
//    std::vector<cv::Point2f> refer_point2fs;
////	getProperReferPoints(refer_point2fs,boxsize,img.size);
//    accuracy refer_width,refer_height;
//    if (couple.boxsize() == rm::BIG_BOX ) {
//        refer_width = BIG_ARMOR_BOX_W;
//        refer_height = BIG_ARMOR_BOX_H;
//
//    }
//    else if(couple.boxsize() == rm::SMALL_BOX ) {
//        refer_width = SMALL_ARMOR_BOX_W;
//        refer_height = SMALL_ARMOR_BOX_H;
//    }
//
////	Point2f center=cv::Point2f(img_size()/2);0
//    cv::Point2f center=cv::Point2f(PICTURE_COL_NUM,PICTURE_ROW_NUM);
//
//    accuracy blob_len=std::max(couple.left_mark.size().x,couple.right_mark.size().x),blob_dis=getPointLength(couple.left_mark.center(),couple.right_mark.center());
//    accuracy coef=blob_len/float(refer_height);
//	refer_width*=coef;
//	refer_height*=coef;
//
//    refer_point2fs.emplace_back(cv::Point2f(0, refer_height));//左下
//    refer_point2fs.emplace_back(cv::Point2f(0 , 0));//左上
//    refer_point2fs.emplace_back(cv::Point2f(refer_width , 0));//右上
//    refer_point2fs.emplace_back(cv::Point2f(refer_width, refer_height));//右下
//
////	Point2f src_points[4];
////	Point2f dst_points[4];
////    cv::Mat rotation = getPerspectiveTransform(box_vertexes, refer_point2fs);  //计算透视变换矩阵
////    (0,0)   _________________  (w,0)
////           |                 |          P=[0,w,w,0;
////           |                 |             0,0,h,h;
////           |                 |             1,1,1,1]
////           |_________________|
////     (0,h)                      (w,h)
//    // typedef Mat_<float> cv::Mat1f
//    cv::Mat1f rotation;
//    cv::findHomography(box_vertexes, refer_point2fs, 1).convertTo(rotation,CV_32F);
//    cv::Mat1f H=rotation;
////  std::cout<<rotation<<std::endl<<std::endl<<H<<std::endl<<std::endl;
////  defined and enlarge the bounding-markerCOUPLE-rect's height to armorbox's size
//    cv::Mat1f p =(cv::Mat_<float>(3,4)<<0.,refer_width,refer_width,0.,-0.5*setter.perspect_num_box_plus_ratio,(-0.5*setter.perspect_num_box_plus_ratio),refer_height*(1+0.5*setter.perspect_num_box_plus_ratio),refer_height*(1+0.5*setter.perspect_num_box_plus_ratio),1.,1.,1.,1.);
//
//    if(rotation.empty()) return cv::Mat();
//    cv::Mat1f P=rotation*p;
//    cv::Mat1f min_cols,max_cols;
//    cv::reduce(P, min_cols, 1, cv::REDUCE_MIN);
//    cv::reduce(P, max_cols, 1, cv::REDUCE_MAX);
////    std::cout<<P<<std::endl<<std::endl;
//
//    float minx=min_cols.at<float>(0),miny=min_cols.at<float>(1);
////    std::cout<<minx<<" "<<miny<<std::endl;
//    float maxx=max_cols.at<float>(0),maxy=max_cols.at<float>(1);
//
//    rotation-=(cv::Mat_<float >(3,3)<<0,0,minx<0?minx:0,0,0,miny<0?miny:0,0,0);
////    std::cout<<rotation<<std::endl;
//    cv::Size2f warp_size=cv::Size2f((maxx-minx),(maxy-miny)*(1+setter.perspect_num_box_plus_ratio));
////    std::cout<<warp_size<<std::endl<<std::endl;
//
//    cv::Rect2f temprect=bounding_rect & cv::Rect2f(0, 0, img.cols, img.rows);
//    if (temprect==cv::Rect2f()) return cv::Mat();
//    cv::Mat tempimg=img(temprect).clone();
//    warpPerspective(tempimg,warp_img, rotation,warp_size,cv::INTER_NEAREST | cv::WARP_FILL_OUTLIERS);
//    cvtColor(warp_img,warp_img, cv::COLOR_BGR2GRAY);
////    equalizeHist(warp_img,warp_img);
//    warp_img*=(float)20.0;
////    imshow("perspect_point",warp_img);
////    showMarkerCouple(show_pic,couple);
////    cv::imshow("show_pic",show_pic);
//////    cv::waitKey(0);
//    if (couple.boxsize() == rm::BIG_BOX ) {
//        cv::Rect2f roi= cv::Rect2f(cv::Point2f(warp_img.cols*0.25,0),cv::Point2f(0.75*warp_img.cols,warp_img.rows))&cv::Rect2f(3,3,warp_img.cols,img.rows);
//        cv::rectangle(show_pic,roi, cv::Scalar(0, 255, 0),1);
//        if (roi==cv::Rect2f()){return cv::Mat();};
//        warp_img=warp_img(roi);
//    }
//
//cv::Mat num_img=warp_img.clone();
//if(setter.save_num_picture){
//static int i=0;
//if((gSrcNum%setter.save_internal)==0){
//std::string num;
//num=Num_Path1+"/picture_"+ std::to_string(gSrcNum) +".jpg";
//cv::imwrite(num,warp_img);
//
//cv::Mat ori=20*origin(bounding_rect & cv::Rect2f(0, 0, show_pic.cols, src.rows));
//num=Num_Path1+"/picture_"+ std::to_string(gSrcNum) +"ori.jpg";
//cv::imwrite(num,ori);
//}
//}
//
//    threshold(num_img, num_img, setter.num_threshold, 255, cv::THRESH_BINARY);
//    medianBlur(num_img, num_img, 2 * 1 + 1);
//
//if(setter.save_num_picture){
//if((gSrcNum%setter.save_internal)==0){
//std::string num;
//num=Num_Path2+"/picture_"+ std::to_string(gSrcNum) +"num.jpg";
//cv::imwrite(num,num_img);
//}
//}
//
//    getsystime(t2);
//    LOGT("boxPerspectiveTransform:%.1fms",getTimeIntervalms(t2,t1));
//    return warp_img;
//}

cv::Mat findNumRoi(const cv::Mat &track_origin,const rm::MarkerCouple &couple)
{
    if(track_origin.empty()) return cv::Mat();
    cv::Rect2f ROI=(couple.getNumBox())&(cv::Rect2f(MAX_FONT_THICKNESS,MAX_FONT_THICKNESS,track_origin.cols-MAX_FONT_THICKNESS,track_origin.rows-MAX_FONT_THICKNESS));
//    cv::rectangle(src,ROI,cv::Scalar(0,255,0),2);
    if(ROI.width==0||ROI.height==0) return cv::Mat();
    return track_origin(ROI);
}

int rm::ArmorFinder::judgeNum(cv::Mat &num_img,const bool is_highland_blind_view){
    cv::resize(num_img, num_img, cv::Size(rm_setter.num_img_size, rm_setter.num_img_size));
    cv::Mat ori=num_img*10;
//        num_img=num_img*10;
    cvtColor(num_img, num_img, cv::COLOR_BGR2GRAY);
    cv::equalizeHist(num_img,num_img);
    cv::threshold(num_img,num_img,rm_setter.num_threshold,255,0);

    systime t1,t2;
    getsystime(t1);
    int num=lenet.forward(num_img,is_highland_blind_view);
    getsystime(t2);
    LOGT("judgeNum:%.1fms",getTimeIntervalms(t2,t1));
    return num;
}

extern cv::Mat show_pic;
rm::Armors rm::ArmorFinder::findArmor(rm::MarkerCouples &couples,const cv::Mat &track2show,const cv::Mat &track_origin,cv::Point2f offset){
    if(couples.size()==0) return Armors();
    Armors armors;
    systime t1,t2;
    getsystime(t1);
    cv::Mat ori;

    for(auto &couple:couples) {
        int real=0;
        cv::Mat num_img=findNumRoi(track_origin,couple);
        if(num_img.empty()) {
            continue;
        }
        int num=judgeNum(num_img,true);
        couple.setOffset(offset);//must after findNumRoi but before pnp
        float refer_pitch=atan((couple.center().y-IMAGE_CENTER_Y) /(FOCUS_PIXAL_8MM*FOCUS))*ANGLE_PER_RAD;
        pnp(couple,offset,show_pic);//limian setPNPCenter
        if(num>0){
            cv::Point3f worldcoord=worldCoordinateResolver(couple.getPNPCenter(),R_vec,serial.qChassis_locations[-1]);
            couple.setState(worldcoord,refer_pitch,1);//include judge blind_flag&&judge state
        }
        else{
#if SHOOTER_MODE==UP_SHOOTER
//        cv::adaptiveThreshold(num_img,num_img,255,0,0);
        cv::Point3f worldcoord=worldCoordinateResolver(couple.getPNPCenter(),R_vec,serial.qChassis_locations[-1]);
            couple.setState(worldcoord,serial.qPitches[-1]+ refer_pitch,2);//include judge blind_flag&&judge state
        couple.setOffset(-offset);//because after pnp
//        float length=std::min(couple.left_mark.size().x,couple.right_mark.size().x);
//        float ratio=(getPointLength(couple.left_mark.center(),couple.right_mark.center()))/(length);
//        std::cout<<(RAIL_HEIGHT+RAIL_DIFF-worldcoord.y)<<std::endl;
//        bool is_highland_blind_height=((RAIL_HEIGHT+RAIL_DIFF-worldcoord.y)<0)?true:false;
//        bool is_highland_blind_view=(ratio>rm_setter.marker_middle_distance_height_ratio)&&is_highland_blind_height&&(abs(worldcoord.z)>rm_setter.highland_blindview_min_distance)&&(abs(worldcoord.z)<rm_setter.highland_blindview_max_distance);
//        if(abs(cos(last_track_armor.couple.getEular()[EULAR_AXIS]-couple.getEular()[EULAR_AXIS]))) real+=5;
//        if(sin(abs(serial.qYaws[-1]+couple.getEular()[EULAR_AXIS]))>0.960) real+=10;

        couple.setBoxsize(judgeBoxSize(couple.getBlindFlag(),couple.left_mark,couple.right_mark));
        num_img=findNumRoi(track_origin,couple);
            if(num_img.empty()) {
                continue;
            }
        couple.setOffset(offset);
//        pre_pic=num_img;
        num=judgeNum(num_img,true);
#endif
        }
        if(rm_setter.show_num_roi) cv::rectangle(show_pic,couple.getNumBox(),cv::Scalar(0,255,0));//must after setboxsize
        if((num==2)&&couple.getBlindFlag()) num=3;
//        int num=judgeNum(num_img,is_highland_blind_view);
        if(num>0) real+=100;
//        if((state==TRACKING_STATE)&&is_highland_blind_view&&(abs(couple.getBoxAngle()-last_track_armor.couple.getBoxAngle())<1.5)) num=(num==0)?(last_track_armor.getID()%7):num;
//        int num=judgeNum(boxPerspectiveTransform(origin,couple));
        if (num != 0 and num < 8) {
            if(num==1) couple.setBoxsize(rm::BIG_BOX);
            else couple.setBoxsize(rm::SMALL_BOX);
            if (couple.color() == rm::RED) num += 7;
            Armor armor(couple,num,real);
            armors.emplace_back(armor);
            if(rm_setter.save_wrong_picture) {
                if((gSrcNum%rm_setter.save_internal)==0){
                    std::string num;
                    num=Wrong_Path+"/picture_"+ std::to_string(gSrcNum) +"prcss.jpg";
                    cv::imwrite(num,num_img);
                    num=Wrong_Path+"/picture_"+ std::to_string(gSrcNum) +"src.jpg";
                    cv::imwrite(num,ori);
                }
            }
            if(rm_setter.show_CSYS){
                pnp.drawCSYS(show_pic);
            }
        }

        if(num == 0&&rm_setter.save_miss_picture) {
            if((gSrcNum%rm_setter.save_internal)==0){
                std::string num;
                num=Miss_Path+"/picture_"+ std::to_string(gSrcNum) +"prcss.jpg";
                cv::imwrite(num,num_img);
                num=Miss_Path+"/picture_"+ std::to_string(gSrcNum) +"src.jpg";
                cv::imwrite(num,ori);
            }
        }
    }
    getsystime(t2);
    if(armors.empty()) LOGW("FIND NO ARMOR!");
    LOGT("findArmor:%.1fms",getTimeIntervalms(t2,t1));
    return armors;
};

extern int lost_then_keep_tracking_cnt;
extern int anti_switch_cnt;
rm::Armor rm::ArmorFinder::decision(rm::Armors &armors,const cv::Mat src){
    if(armors.size()==0) return Armor();
    //	armor_boxes.size();
    // 按照优先级对装甲板进行排序

    systime t1,t2;
    getsystime(t1);
    if (armors.size() >= 2) {
        sort(armors.begin(),armors.end(), [&](const rm::Armor &a, const Armor &b) {
            double dis_a, dis_b,angle_a,angle_b;
            if (last_track_armor.couple.getNumBox() != cv::Rect2f()) {
                angle_a = abs(a.couple.getBoxAngle() - last_track_armor.couple.getBoxAngle());
                angle_b = abs(b.couple.getBoxAngle() - last_track_armor.couple.getBoxAngle());
                dis_a=getPointLength(a.center() - last_track_armor.center());
                dis_b=getPointLength(b.center() - last_track_armor.center());
//                     std::cout<<"dis_b.getrodetoEularVec()[2]"<<a.box_pos.getrodetoEularVec()[2]<<std::endl;
//                     dis_a=abs(a.box_pos.getrodetoEularVec()[2] - last_armor.box_pos.getrodetoEularVec()[2]);
//                     dis_b=abs(b.box_pos.getrodetoEularVec()[2] - last_armor.box_pos.getrodetoEularVec()[2]);
                     }
                 else{
                     dis_a=(getPointLength(a.center() - cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y)));
                     dis_b=(getPointLength(b.center() - cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y)));
                 }


            //when the higher priority level target is not so far from the center,choose the higher priority one;
            //else choose the closer one
//            if(a.getID()==b.getID()) {
//                if (last_track_armor.couple.getNumBox() != cv::Rect2f()) {
////                    cv::putText(show_pic,"AAA",cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,2,cv::Scalar(150,255,255),3);
//                   if(rm_setter.print_state_switch_test_info) LOGE("angle_a:%lf angle_b:%lf",angle_a,angle_b);
//                    return angle_a<angle_b;
//                }
//                else{
//                    if(rm_setter.print_state_switch_test_info) LOGE("dis_a:%lf dis_b:%lf",dis_a,dis_b);
//                    return abs(angle_a)<abs(angle_b);
//                }
//            }
//id is same
                 if(a.getID()==b.getID()) {
                     if (last_track_armor.couple.getNumBox() != cv::Rect2f()) {
//                    cv::putText(show_pic,"AAA",cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,2,cv::Scalar(150,255,255),3);
                         if(rm_setter.print_state_switch_test_info) LOGE("angle_a:%lf angle_b:%lf",angle_a,angle_b);
                         return angle_a<angle_b;
                     }
                     else{
                         if(rm_setter.print_state_switch_test_info) LOGE("dis_a:%lf dis_b:%lf",dis_a,dis_b);
                         return abs(a.couple.getBoxAngle())<abs(b.couple.getBoxAngle());
                     }
                 }
                 //id not same
                if(last_track_armor.getID()== -1||((std::max(dis_a,dis_b))<2000)||((std::min(dis_a,dis_b))>=2000)) {
                    if((a.getID()%7)!=1&&(b.getID()%7)!=1){
                        if(last_track_armor.getID()==a.getID()) return true;
                        else if(last_track_armor.getID()==b.getID()) return false;
                    }
                    else return a<b;
                }
                else  return dis_a < dis_b;
//                 if (last_armor.couple.getNumBox()!= cv::Rect2f()) return (getPointLength(a.center() - last_armor.center()) < getPointLength(b.center() - last_armor.center()));
//                 else return a < b;
             }
        );
        if(rm_setter.print_state_switch_test_info) {
            for(int i=0;i<armors.size();i++)
                if(rm_setter.print_state_switch_test_info) LOGM("Armor %d: %d",i+1,armors[i].getID());
        }
    }

    rm::Armor box,one_box;
        for (auto &one_box : armors) {
        if (one_box.getID() != 0) {
#if SHOOTER_MODE == DOWN_SHOOTER
#ifndef ASSISTED_FIRE
              if((one_box.getID() == serial.rxpackage.another_shoot_id) && armors.size()>1) continue;
#endif
#endif
#if SHOOT_ENGINEER==0
            if(one_box.getID()!=2&&one_box.getID()!=9){
		        box = one_box;
			    continue;
		    }
#else
            box = one_box;
            break;
#endif
        }
    }
#ifndef ASSISTED_FIRE
if(armors[0].getID()==serial.rxpackage.another_shoot_id&&box.getID()==-1) box=armors[0];//
#endif
    bool is_same_enemy = bool((box.getID() == last_track_armor.getID())&&(last_track_armor.getID()!=-1));
    bool is_same_box = bool(is_same_enemy&&abs(box.couple.getBoxAngle()-last_track_armor.couple.getBoxAngle())<rm_setter.change_armor_angle);
//    char flag[100];
//    sprintf(flag,"%d", box.getID() == last_track_armor.getID());
//    cv::putText(show_pic,flag,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,2,cv::Scalar(150,255,255),3);
    if(last_track_armor.getID()== -1||is_same_box||(is_same_enemy&&!is_same_box&&lost_then_keep_tracking_cnt+box_anti_switch_cnt++>2)||(!is_same_enemy)&&anti_switch_cnt++>rm_setter.anti_switch_cnt_limit){
        if(is_same_enemy) anti_switch_cnt=0;
        box_anti_switch_cnt=0;
        cv::circle(show_pic,box.center(),4,cv::Scalar(255, 255, 255),2);
        cv::putText(show_pic,"SHOOT",cv::Point2f(box.center().x-15,box.center().y-30),1,2,cv::Scalar(150,255,255),3);
        getsystime(t2);
        LOGT("decision:%.1fms",getTimeIntervalms(t2,t1));
        return box;
    }
    else return Armor();

}//数字识别（不同pb文件）(截好图再投)

rm::Armor rm::ArmorFinder::gyrodecision(rm::Armors &armors,const cv::Mat src){
    if(armors.size()==0) return Armor();
    //	armor_boxes.size();
    // 按照优先级对装甲板进行排序

    systime t1,t2;
    getsystime(t1);
    if (armors.size() >= 2) {
        sort(armors.begin(),armors.end(), [&](const rm::Armor &a, const Armor &b) {
                 double angle_a,angle_b;

                     if (last_track_armor.couple.getNumBox() != cv::Rect2f()) {
                         angle_a = abs(a.couple.getBoxAngle() - last_track_armor.couple.getBoxAngle());
                         angle_b = abs(b.couple.getBoxAngle() - last_track_armor.couple.getBoxAngle());
//                    cv::putText(show_pic,"AAA",cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,2,cv::Scalar(150,255,255),3);
                         return angle_a<angle_b;
                     }
                     else{
                         if(rm_setter.print_state_switch_test_info) LOGE("angle_a:%lf angle_b:%lf",angle_a,angle_b);
                         return abs(a.couple.getBoxAngle())<abs(b.couple.getBoxAngle());
                     }
        }
        );
    }

    rm::Armor box,one_box;
    for (auto &one_box : armors) {
        if (one_box.getID() != 0) {
            box = one_box;
            break;
        }
    }
    bool is_same_box = bool((box.getID() == last_track_armor.getID())&&last_track_armor.getID()!=-1&&abs(box.couple.getBoxAngle()-last_track_armor.couple.getBoxAngle())<rm_setter.change_armor_angle);
//    char flag[100];
//    sprintf(flag,"%d", box.getID() == last_track_armor.getID());
//    cv::putText(show_pic,flag,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,2,cv::Scalar(150,255,255),3);
    if(last_track_armor.getID()== -1||is_same_box||(!is_same_box&&lost_then_keep_tracking_cnt+box_anti_switch_cnt++>2)){
        box_anti_switch_cnt=0;
        cv::circle(show_pic,box.center(),4,cv::Scalar(255, 255, 255),2);
        cv::putText(show_pic,"SHOOT",cv::Point2f(box.center().x-15,box.center().y-30),1,2,cv::Scalar(150,255,255),3);
        getsystime(t2);
        LOGT("decision:%.1fms",getTimeIntervalms(t2,t1));
        return box;
    }
    else return Armor();
}


